/*
  BenderTestSuite
  Test suite to test functionality of Bender electronics
*/

// #include <Max3421e.h>
// #include <Usb.h>
// #include <AndroidAccessory.h>

#define ON 1
#define OFF 0

#define CW 0
#define CCW 1

#define MOTOR_ENABLE 19
#define MOTOR_CLOCKWISE 12
#define MOTOR_COUNTER_CLOCKWISE 13

#define HALL_GS 2
#define HALL1 18
#define HALL2 20
#define HALL3 3

#define BIG_RED_BUTTON 21

#define VALVE_ENABLE 36
#define VALVE1 46
#define VALVE2 48
#define VALVE3 34

#define LED_ENABLE 44
#define LED1 42
#define LED2 38
#define LED3 40

int motorSpeed = 255;

int lastDirection = CW;

int buttonState = 0;

int activeHallEffect = -1;
int lastHallEffect = 0;

int LEDBrightness = 0;

//AndroidAccessory acc("SOMA",
//                     "Bender",
//                     "Bender Arduino Board",
//                     "1.0",
//                     "http://code.google.com/p/vader-robot/",
//                     "0000000012345678");

void setup() {
    // open the serial port:
  Serial.begin(115200);

  // put your setup code here, to run once:
  pinMode(MOTOR_ENABLE, OUTPUT);
  pinMode(MOTOR_CLOCKWISE, OUTPUT);
  pinMode(MOTOR_COUNTER_CLOCKWISE, OUTPUT);
  digitalWrite(MOTOR_ENABLE, LOW);   //Default motor enable to LOW
  
  pinMode(HALL_GS, INPUT);
  pinMode(HALL1, INPUT);
  pinMode(HALL2, INPUT);
  pinMode(HALL3, INPUT);
  attachInterrupt(0, magnetPresent, CHANGE);
  
  pinMode(BIG_RED_BUTTON, INPUT);
  attachInterrupt(2, buttonPressed, RISING);
  buttonState = 0;
  
  pinMode(VALVE_ENABLE, OUTPUT);
  pinMode(VALVE1, OUTPUT);
  pinMode(VALVE2, OUTPUT);
  pinMode(VALVE3, OUTPUT);
  // Disable valves by default
  digitalWrite(VALVE_ENABLE, HIGH);
  
  pinMode(LED_ENABLE, OUTPUT);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);
  // Disable LED by default
  analogWrite(LED_ENABLE, 255);
  
//  acc.powerOn();
  printInstructions();
}

void printInstructions(){
  Serial.println("Welcome to the Bender Test Suite");
  Serial.println("  h: Print this"); 
  Serial.println("  q: Turn Motor Clockwise");
  Serial.println("  w: Turn Motor Counter-Clockwise");
  Serial.println("  e: Stop the motor");
  Serial.println("  r: Decrease speed of motor by 10\%");
  Serial.println("  t: Increase speed of motor by 10\%");
  Serial.println("  a: Get values of hall effect encoder");
  Serial.println("  0-8: Enable Valve n, 8 turns them off");
  Serial.println("  z-,: Enable LED n, l turns them off");
}

void magnetPresent(){
  int a_zero_state = digitalRead(HALL1);
  int a_one_state = digitalRead(HALL2);
  int a_two_state = digitalRead(HALL3);
  activeHallEffect = (!a_two_state << 2) | (!a_one_state << 1) | (!a_zero_state);
}

void buttonPressed(){
  buttonState = 1;
}

void MotorCtrl(int motorDirection, int motorCtrlSpeed){
  if(motorCtrlSpeed > 0){
    digitalWrite(MOTOR_ENABLE, HIGH);
  }
  else{
    digitalWrite(MOTOR_ENABLE, LOW);
    Serial.println("Motor Off");
    return;
  }
  
  if(motorDirection == CW){
    Serial.print("Turning CW at ");
    Serial.println(motorCtrlSpeed);
    analogWrite(MOTOR_CLOCKWISE, motorCtrlSpeed);
    digitalWrite(MOTOR_COUNTER_CLOCKWISE, LOW);
  }
  else if(motorDirection == CCW){
    Serial.print("Turning CCW at ");
    Serial.println(motorCtrlSpeed);
    digitalWrite(MOTOR_CLOCKWISE, LOW);
    analogWrite(MOTOR_COUNTER_CLOCKWISE, motorCtrlSpeed);
  }
}

void printEncoder(){
  int gs_state = digitalRead(HALL_GS);
  int a_zero_state = digitalRead(HALL1);
  int a_one_state = digitalRead(HALL2);
  int a_two_state = digitalRead(HALL3);
  Serial.print("GS: ");
  Serial.println(gs_state);
  Serial.print("A0: ");
  Serial.println(a_zero_state);
  Serial.print("A1: ");
  Serial.println(a_one_state);
  Serial.print("A2: ");
  Serial.println(a_two_state);
}

void openValve(int valve) {
  digitalWrite(VALVE_ENABLE, LOW);
  Serial.print("Opening Valve ");
  Serial.println((valve & 0x07));
  digitalWrite(VALVE1, valve & 0x01 ? HIGH : LOW);
  digitalWrite(VALVE2, valve & 0x02 ? HIGH : LOW);
  digitalWrite(VALVE3, valve & 0x04 ? HIGH : LOW);
}
void closeValves() {
  Serial.println("Closing valves");
  digitalWrite(VALVE_ENABLE, HIGH);
}

void turnOnLED(int led, int brightness) {
  analogWrite(LED_ENABLE, brightness);
  digitalWrite(LED1, led & 0x01 ? HIGH : LOW);
  digitalWrite(LED2, led & 0x02 ? HIGH : LOW);
  digitalWrite(LED3, led & 0x04 ? HIGH : LOW);
} 

void loop() {
  //byte msg[6];
  //if (acc.isConnected()) {
  //  // Check for messages from android.
  //  int len = acc.read(msg, sizeof(msg), 1);
  //  if (len > 0) {
  //    Serial.print("Recieved message, length ");
  //    Serial.println(len, DEC);
  //    for(int i=0; i < len; i++) {
  //      Serial.print(msg[i], HEX);
  //      Serial.print(" ");
  //    }
  //    Serial.println("");
  //  }
  //}
  // check for incoming serial data:
  if (Serial.available() > 0) {
    // read incoming serial data:
    char input = Serial.read();
    int led = 0;
    // Type the next ASCII value from what you received:
    switch(input){
      case 'h':
        printInstructions();
        break;
      case 'q':
        //CW Motor
        MotorCtrl(CW,motorSpeed);
        lastDirection = CW;
        break;
      case 'w':
        //CCW Motor
        MotorCtrl(CCW,motorSpeed);
        lastDirection = CCW;
        break;
      case 'e':
        //Stop Motor
        MotorCtrl(lastDirection, 0);
        break;
      case 'r':
        //Slow Motor
        motorSpeed = motorSpeed - 10;
        if(motorSpeed < 0){
          motorSpeed = 0;
        }
        MotorCtrl(lastDirection, motorSpeed);
        break;
      case 't':
        //Slow Motor
        motorSpeed = motorSpeed + 10;
        if(motorSpeed > 255){
          motorSpeed = 255;
        }
        MotorCtrl(lastDirection, motorSpeed);
        break;
      case 'a':
        printEncoder();
        break;
      case '0':
      case '1':
      case '2':
      case '3':
      case '4':
      case '5':
      case '6':
      case '7':
        openValve(input - '0');
        break;
      case '8':
        closeValves();
        break;
      case '\,': led++;
      case 'm':  led++;
      case 'n':  led++;
      case 'b':  led++;
      case 'v':  led++;
      case 'c':  led++;
      case 'x':  led++;
      case 'z':  
        turnOnLED(led, LEDBrightness);
        break;
      case 'l':
        turnOnLED(0, 255);
        break;
      case 'j':
        LEDBrightness = LEDBrightness - 10;
        if(LEDBrightness < 0){
          LEDBrightness = 0;
        }
        Serial.print("Decreasing brightness to ");
        Serial.println(LEDBrightness);
        analogWrite(LED_ENABLE, LEDBrightness);
        break;
      case 'k':
        LEDBrightness = LEDBrightness + 10;
        if(LEDBrightness > 255){
          LEDBrightness = 255;
        }
        Serial.print("Increasing brightness to ");
        Serial.println(LEDBrightness);
        analogWrite(LED_ENABLE, LEDBrightness);
        break;
      case '\n':
        break;
      default:
        Serial.println("Unrecognized command");
        printInstructions();
        break;
    }
  }
  if(buttonState == 1){
    Serial.println("Button Pressed!");
    buttonState = 0;
  }
  if(activeHallEffect >= 0){
    int gs_state = digitalRead(HALL_GS);
    if(gs_state == HIGH){
      Serial.print("Removed Hall from ");
      Serial.println(lastHallEffect);
    }
    else{
      Serial.print("Detected Hall ");
      Serial.println(activeHallEffect);
      lastHallEffect = activeHallEffect;
    }
    activeHallEffect = -1;
  }
}
